#include "WPILib.h"
#include "Joysticks.hpp"
#include "RobotDrive2012.hpp"
#include "math.h"
#include "Shooter.hpp"
//#include "smartGyro4.cpp"
#include "simplegyro.hpp"
#include "antislip.hpp"
#include "Scaler.hpp"
#include "Jogger.hpp"
#include "Stepper.hpp"
#include "Gate.cpp"

// NOTE: Shooter instanciation reequires myShooter to be a pointer

/*		XBox 360 specific controls
 *
 * 
 *  	
 	   GetLeftStickX();//Get X value for given axis
	   GetLeftStickY();//Get Y value for given axis
	   GetRightStickX();//Get X value for given axis
	   GetRightStickY();//Get Y value for given axis
	   GetDPad();//Get DPad value
	   GetDPadX();//Get DPad X value (-1,0,1)
	   GetDPadY();//Get DPad Y value (-1,0,1)
	   GetTrigger();//Get Trigger value (-1,0,1)
	   GetLeftTrigger();// Get value for given button
	   GetRightTrigger();// Get value for given button
	   GetButtonA();// Get value for given button
	   GetButtonB();// Get value for given button
	   GetButtonX();// Get value for given button
	   GetButtonY();// Get value for given button
	   GetButtonLeftBumper();// Get value for given button
	   GetButtonRightBumper();// Get value for given button
	   GetButtonBack();// Get value for given button
	   GetButtonStart();// Get value for given button
	   GetButtonLeftStick();// Get value for given button
	   GetButtonRightStick();// Get value for given button
 * 
 * 
 */
//	add the following code segments ***********************************
#define DEFINES
#define FEEDER
#define SHOOTER
#define GATE
//#define BRIDGE
#define AUTONOMOUS_SHOOT
//#define AUTONOMOUS
//*********************************************************************

//--- Set All Controls Here !!! ***************************************
// Shooter Mode
#define XDIRECTION -1.0*
#define YDIRECTION -1.0*
#define DEFAULTFEEDSPEED 0.5
#define DEFAULTUNLOADSPEED -0.5
#define DRIVE_FORWARD_REVERSE stick.GetLeftStickY()
#define DRIVE_ROTATE stick.GetLeftStickX()
#define DRIVE_SLOW stick.GetButtonRightBumper()
#define DRIVE_STOP stick.GetButtonLeftBumper()
#define SHOOT_BALL stick.GetButtonRightBumper()
#define TARGETING_CONTROL 0.64*stick.GetRightStickY()
#define GATE_CONTROL stick.GetButtonX()
//#define TARGETING_CONTROL 0.72
#define LOAD_BALLS stick.GetButtonY()
#define STOP_BALL_LOADER stick.GetButtonB()
#define UNLOAD_BALLS stick.GetButtonA()
#define ENTER_BALLANCE_MODE stick.GetButtonStart()

// Ballance Mode
//#define DRIVE_FORWARD_REVERSE stick.GetLeftStickY()  - Defined in Shooter Mode
//#define DRIVE_ROTATE stick.GetLeftStickX()
//#define DRIVE_STOP stick.GetButtonLeftBumper()
#define BRIDGEARMUP stick.GetButtonA()
#define BRIDGEARMDOWN stick.GetButtonY()
#define ZERO_BALLANCE_GYRO stick.GetButtonStart()
#define EXIT_BALLANCE_MODE stick.GetButtonBack()
#define ROCKER_MODE_DIRECTION -stick.GetRightStickY()
#define STEP_MODE_DIRECTION -stick.GetRightStickY()
#define INITIATE_ROCKER_MODE stick.GetButtonRightStick()
#define INITIATE_STEP_MODE stick.GetButtonLeftStick()

//*********************************************************************



//#define AUTONOMOUS
#define AUTONOMOUS_SHOOT
//#define AUTONOMOUS_BRIDGE
#define JAGUARS
#define BRAKE_HARD
#define TEMPSHOOTER

#define AUTO_SHOOT_SPEED 0.72
#define AUTO_SHOOT_TIME .75
#define AUTO_TRAVEL_SPEED .5
#define AUTO_TRAVEL_TIME 5
#define AUTO_WAIT_FOR_MOTOR 3
#define AUTO_BETWEEN_SHOOT_TIME 2

// Channel asignments *************************************************
#define SHOOTERCHANNEL 7
#define CAMERACHANNEL 3
#define SIDEFEEDERCHANNEL 10
#define BOTTOMFEEDERCHANNEL 11
#define GCHANNEL 1
#define BRIDGEARMCHANNEL 8
//#define BRIDGEARMCHANNEL 11 //Temp
//*********************************************************************

#define WAITTIME 0.005
#define SHOOTER_DEADBAND 0.25


#define DEAD_SPACE  0.15
#define ACCX  0.05
#define ACCY  0.05

#define YBALLANCEFACTOR 0.5
#define XBALLANCEFACTOR 0.5
#define DEFAULTROCKSPEED 0.2
#define DEFAULTSTEPPERSPEED 0.2
#define GZHOLDMULTP -0.3*0.8/18
#define GZHOLDMULTN -0.3*0.6/12
#define GZHOLDLIMITN 0.45*0.8
#define GZHOLDLIMITP 0.45*0.8
#define MAXANGLE 16.0
#define MINANGLE 16.0
#define DEFAULTSTEPTIMEMOVE .05
#define DEFAULTSTEPTIMESTOP 1
#define DEFAULTROCKTIME .1
#define BRIDGEHARDSTOP 1.0
#define BRIDGETIMECOUNT 100
#define BRIDGEDOWNPOSITION -1
#define BRIDGEUPPOSITION 1
#define BRIDGEUPPOSITIONHOLD 0.2
#define BRIDGEDOWNPOSITIONHOLD -0.75
#define BRIDGEVOLTAGERAMP 3.0
#define BRIDGEMAXVOLTAGE 12.0
#define BRIDGEDOWNCOUNT 600
#define BRIDGEUPCOUNT 600
#define BRIDGEACTIVETIME 5

/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */ 
class RobotDemo : public SimpleRobot
{
	//RobotDrive myRobot; // robot drive system
	XBox360 stick; // only joystick
	RobotDrive2012 myRobot2012;
#ifdef SHOOTER
	Shooter* myShooter;
#endif
	
#ifdef FEEDER
	CANJaguar  mySideFeeder
	,myBottomFeeder
#endif
	
	;
#ifdef GATE
	Gate myGate;
#endif
	
	Gyro GZHold;
#ifdef BRIDGE
	CANJaguar BridgeArm;
#endif
#ifdef FEEDER
	float FeederMode;
#endif

//	float FeederMode;

public:
	RobotDemo(void):
		stick(1)		// as they are declared above.
		,myRobot2012()
		
		
#ifdef FEEDER
		,mySideFeeder(SIDEFEEDERCHANNEL)
		,myBottomFeeder(BOTTOMFEEDERCHANNEL)
#endif
		
#ifdef GATE
		,myGate()
#endif

		,GZHold(GCHANNEL)
#ifdef BRIDGE
	,BridgeArm(BRIDGEARMCHANNEL,CANJaguar::kPercentVbus)
#endif

	{
		printf("Entering Constructor\n");
		myRobot2012.SetExpiration(3);
		myRobot2012.SetSafetyEnabled(false);
		myRobot2012.SetNeutral(CANJaguar::kNeutralMode_Brake);
#ifdef BRIDGE
		BridgeArm.ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
		BridgeArm.EnableControl();
		BridgeArm.ConfigFaultTime(3);
		BridgeArm.ConfigEncoderCodesPerRev(250);
		BridgeArm.ConfigMaxOutputVoltage(BRIDGEMAXVOLTAGE);
		BridgeArm.SetVoltageRampRate(BRIDGEVOLTAGERAMP);
#endif
#ifdef FEEDER
		FeederMode=0;
#endif
#ifdef SHOOTER
		myShooter = new Shooter(CAMERACHANNEL,SHOOTERCHANNEL);
#endif
		printf("Robot2012\n");
	}

	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
	void Autonomous(void)
	{
#ifdef AUTONOMOUS
		//myRobot.SetSafetyEnabled(false);
		//myRobot.Drive(0.5, 0.0); 	// drive forwards half speed
		//Wait(2.0); 				//    for 2 seconds
		//myRobot.Drive(0.0, 0.0); 	// stop robot
		
		myRobot2012.SetSafetyEnabled(false);
		myRobot2012.ArcadeDrive(0.0,AUTO_SHOOT_SPEED);
		
#ifdef AUTONOMOUS_SHOOT
		int iCnt1=600;
		float GtCtrl=GATEHOLD;
		bool bFlg1=true,bFlg2=false;
		while(IsAutonomous ())
			{

		
		myShooter->ManualControl(AUTO_SHOOT_SPEED);
//		Wait(AUTO_WAIT_FOR_MOTOR);	
		if(iCnt1<0&&bFlg1)
		{
			GtCtrl=-AUTO_TRAVEL_SPEED;
		bFlg1=false;
		bFlg2=true;
		iCnt1=200;
		GtCtrl=AUTO_TRAVEL_SPEED;
		}
		if (iCnt1<0&&bFlg2)
		{
			bFlg2=false;
			GtCtrl=-GATEHOLD;
			mySideFeeder.Set(-DEFAULTFEEDSPEED);
			myBottomFeeder.Set(DEFAULTFEEDSPEED);

		}
		else
		{
			GtCtrl=GATEHOLD;

		}
		
		iCnt1--;
		myGate.Set(GtCtrl);
			
/*		myGate.Set(GATEDRIVE);
		Wait(GATESTARTTIME*-1);
		myGate.Set(-GATEDRIVE);
		Wait(GATEENDTIME);
		myGate.Set(0);
		Wait(AUTO_BETWEEN_SHOOT_TIME);
		myGate.Set(GATEDRIVE);
		Wait(GATESTARTTIME*-1);
		myGate.Set(-GATEDRIVE);
		Wait(GATEENDTIME);
		myGate.Set(0);
*/
			}
#endif
#ifdef AUTONOMOUS_BRIDGE
		BridgeJag.Set(1);
		myRobot2012.Drive(AUTO_TRAVEL_SPEED, 0);
		Wait(AUTO_TRAVEL_TIME);
#endif

#endif

		

	}
	void Disabled()
	{
		while (IsDisabled())
		{
			
			Wait(WAITTIME);	
			myRobot2012.ArcadeDrive(0,0);
		}
	}
	void RobotInit()
	{
		
		while (IsDisabled())
		{
			
			Wait(WAITTIME);	
			myRobot2012.ArcadeDrive(0,0);
		}
	}
	void OperatorControl(void)
	{
		myRobot2012.SetSafetyEnabled(false);
		SimpleDeadband DeadBand(0.15);
		toggle Toggle , Toggle2, Toggle3,Toggle4;

		float YDrive;			// raw value before deadband
		float XDrive;			// raw value before deadband
		scaler AccelerationCtrlX(ACCX),AccelerationCtrlY(ACCY);
		bool IsBallanceMode=false;
		myRobot2012.SetNeutral(CANJaguar::kNeutralMode_Brake);
		Stepper Step(DEFAULTSTEPTIMEMOVE,DEFAULTSTEPTIMESTOP,DEFAULTSTEPPERSPEED);
		Jogger Rock(DEFAULTROCKTIME,DEFAULTROCKTIME,DEFAULTROCKSPEED);

#ifdef BRIDGE
		float HoldBridgeArm=0;
		bool bUpFlg=false;
		bool bDnFlg=false;
		float value=-BRIDGEUPPOSITIONHOLD;
#endif
		while (IsOperatorControl())
		{
//			float XDrive=0,YDrive=0;
			// set the mode when a jaguar is in neutral (fed a value of zero)
			
			// Get the raw values from the joystick
#ifdef BRIDGE
			float HoldBridgeArm;
#endif
				
			YDrive =  DRIVE_FORWARD_REVERSE;
			XDrive =  DRIVE_ROTATE;
			// Add acceleration control so the robot doesn't tip over	
			if (DRIVE_SLOW)
			{
				YDrive = YDrive*0.5;
				XDrive = XDrive*0.5;
			}					

//			AccelerationCtrlX.GetSpeed(XDrive);
//			AccelerationCtrlY.GetSpeed(YDrive);
			DeadBand.ModifyValue(XDrive);
			DeadBand.ModifyValue(YDrive);

				
				// BEGIN Semi auto tilt control
				if(!IsBallanceMode)IsBallanceMode=ENTER_BALLANCE_MODE;
				if(IsBallanceMode)
					{
					IsBallanceMode=!EXIT_BALLANCE_MODE;
					}
	
					if ( IsBallanceMode)
					{
						mySideFeeder.Set(0.0);
						myBottomFeeder.Set(0.0);
#ifdef SHOOTER
						myShooter->ManualControl(0.0);
#endif

#ifdef BRIDGE

						if(BRIDGEARMUP||bUpFlg)
						{
							BridgeArm.Set(BRIDGEUPPOSITION);
							if(!bUpFlg)HoldBridgeArm=BRIDGETIMECOUNT;
							bUpFlg=true;
							HoldBridgeArm--;
							if(HoldBridgeArm<0)bUpFlg=false;
							value=-BRIDGEUPPOSITION;
						}
						else if(-BRIDGEARMDOWN||bDnFlg)
						{
							BridgeArm.Set(BRIDGEDOWNPOSITION);
							if(!bDnFlg)HoldBridgeArm=BRIDGETIMECOUNT;
							bDnFlg=true;
							HoldBridgeArm--;
							if(HoldBridgeArm<0)bDnFlg=false;
							value=BRIDGEDOWNPOSITION;

						}
						else
						{
							if(value==BRIDGEUPPOSITION)value=BRIDGEUPPOSITIONHOLD;
							if(value==BRIDGEDOWNPOSITION)value=BRIDGEDOWNPOSITIONHOLD;
							bDnFlg=false;
							bUpFlg=false;
						}
							BridgeArm.Set(value);							
#endif
#ifdef BALLANCE
						if(ZERO_BALLANCE_GYRO)GZHold.Reset();
						YDrive = YDrive*YBALLANCEFACTOR;
//						XDrive =XDrive*XBALLANCEFACTOR;
									printf("    XDrive= %f, YDrive=%f\n",XDrive,YDrive);
			
						if(INITIATE_STEP_MODE)
						{
							Step.Restart( YDIRECTION STEP_MODE_DIRECTION);
							XDrive=0;
							YDrive=Step.GetCurrentSpeed(YDrive);

						}
						 if(INITIATE_ROCKER_MODE)
						{
							Rock.Restart( YDIRECTION ROCKER_MODE_DIRECTION);
							XDrive=0;
							YDrive=Rock.GetCurrentSpeed(YDrive);

						}
						 float tAngle=GZHold.GetAngle();
						 if(tAngle>MAXANGLE)tAngle=MAXANGLE;
						 if(tAngle< -MAXANGLE)tAngle=-MAXANGLE;
						 if(tAngle<0.0)tAngle=tAngle*GZHOLDMULTN;
						 else tAngle=tAngle*GZHOLDMULTP;
						 YDrive=YDrive+tAngle;
//							printf("A   XDrive= %f, YDrive=%f\n",XDrive,YDrive);

							if(YDrive>GZHOLDLIMITP)YDrive=GZHOLDLIMITP;// Limit thhe force to prevent runaway
							if(YDrive<-GZHOLDLIMITN)YDrive=-GZHOLDLIMITN;// Limit thhe force to prevent runaway
//							printf("B   XDrive= %f, YDrive=%f  \n",XDrive,YDrive);
#endif
							
				}
					else
				{
						GZHold.Reset();
			//**************************************************************

					// Gate Drive**********************************************
#ifdef GATE
	myGate.Enable(myShooter->IsSafe());
	myGate.Run();
	if(GATE_CONTROL)myGate.Start();
#endif
					//*************************************************
					
					// Begin Shooter **********************************
#ifdef SHOOTER
					float cameraY = TARGETING_CONTROL;
					
					if(fabs(cameraY) > SHOOTER_DEADBAND)	// temp hard coded deadband
					{
//						printf("Calling myShooter.ControlByCamera - cameraY = %0.2f\n", cameraY);
						myShooter->ControlByCamera(cameraY);
					}
#endif
				}

					if (DRIVE_STOP)
					{
						YDrive = 0;
						XDrive = 0;
					}					

						
					myRobot2012.ArcadeDrive(XDIRECTION XDrive,YDIRECTION YDrive);

					// BEGIN - Feeder Control	
#ifdef FEEDER					
					if(LOAD_BALLS)FeederMode=DEFAULTFEEDSPEED;
					if(STOP_BALL_LOADER)FeederMode=0.0;
					if(UNLOAD_BALLS)FeederMode=DEFAULTUNLOADSPEED;
					mySideFeeder.Set(-FeederMode);
					myBottomFeeder.Set(FeederMode);
				
#endif				

					//*************************************************
			Wait(WAITTIME);				// wait for a motor update time
		}

	}

};

START_ROBOT_CLASS(RobotDemo);

